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Abstract — Planning trajectories for nonholonomic systems is 
difficult and computationally expensive. When facing unexpected 
events, it may therefore be preferable to deform in some way the 
initially planned trajectory rather than to re-plan entirely a new 
one. We suggest here a method based on affine transformations 
to make such deformations. This method is exact and fast: 
the deformations and the resulting trajectories can be com- 
puted algebraically, in one step, and without any trajectory re- 
integration. To demonstrate the possibilities offered by this new 
method, we use it to derive position and orientation correction 
algorithms for the general class of planar wheeled robots and 
for a tridimensional underwater vehicle. These algorithms allow 
in turn achieving more complex applications, including obstacle 
avoidance, feedback control or gap filling for sampling-based 
kinodynamic planners. 

Index Terms — Nonholonomic Motion Planning, Kinematics, 
Wheeled Robots, Marine Robotics 



I. Introduction 

A bicycle, a car, an aircraft, or a submarine are but a 
few examples of nonholonomic systems. Planning trajecto- 
ries for such systems is difficult because, by nature, some 
of their degrees of freedom can only be controlled in a 
coupled manner (see e.g. II 1311 and references therein). As a 
consequence, when such systems encounter on their ways an 
unexpected event (e.g. a random perturbation of the system 
state or of the target state, an unforeseen obstacle, etc.), it 
may be more efficient to deform in some manner the initially 
plan ned traj ectory rather than to re-plan entirely a new one 



Lamiraux and colleagues Ill2ll suggested to iteratively de- 
form the original path by perturbing infinitesimally the control 
inputs at each iteration. However, as underlined by Seiler 
and colleagues 11911 . that method requires re-integrating the 
whole trajectory at each iteration, which is computationally 
expensive. These authors then described a new method based 
on Lie group symmetries, which requires re-integrating only 
parts of the trajectory. 

The Lie groups considered in |i , 19 ] are in fact Euclidean 
(or isometry) groups. We propose here to use larger Lie group s, 
namely, affine groups, which contain the Euclidean transfor- 
mations as subgroups. Using affine transformations allows 
making more versatile trajectory corrections. In particular, the 
corrections are exact and can be computed algebraically, in 



one step, which makes iterative deformations IU2I1 or gradient 
search 14, 191 unnecessary. Furthermore, there is no need to 
re-integrate even a part of the trajectory. Note that, in contrast 
with previous works where the studied systems are invariant 
under Euclidean transformations 0, Il9ll . here trajectories 
and control inputs are not in general affine-invariant. More 
technical precautions need therefore to be taken to define and 
guarantee the feasibility (or admissibility) of the deformed 
trajectories under the system nonholonomic constraints. In 
particular, the admissibility conditions are formulated using 
differential equations with discontinuous right-hand sides 0. 

In section HH we present the general framework of affine 
trajectory correction. We then apply this framework to derive 
position and orientation correction algorithms for two classical 
examples in nonholonomic mobile robotics: the general class 
of planar wheeled robots (sections UTTb and a tridimensional 
underwater vehicle (section HW In section [V] we study some 
more elaborate applications including trajectory correction for 
a car towing trailers, obstacle avoidance, feedback control and 
gap-filling techniques. Finally, in section I VII we discuss the 
advantages and drawbacks of the presented method, its domain 
of applicability, and possible future developments. 

A preliminary version lfl7ll of the present manuscript de- 
scribing position and orientation correction algorithms for the 
unicycle, the bicycle and an underwater vehicle was accepted 
for presentation at the conference Robotics: Science and 
Systems 201 1. 



II. General framework 

A. Affine spaces and affine transformations 

An affine space is a set A together with a group action of 
a vector space W. An element w G W transforms a point 
P G A in to an other point P' by P' = P + w, which can also 
be noted PP' = w. 

Given a point O G A (the origin), an affine transformation T 
of the affine space can be defined by a couple (w, A4) where 
w G W and M. is a non-singular endomorphism of W (i.e. a 
non-singular linear application W — > W). The transformation 
T operates on A by 

VPgA T{P) = + M(UP) +w. 
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Note that, if Pq is a fixed-point of T, then T can be written 
in the form 

VP G A T(P) = P a + M(PqP). 



B. Admissible trajectories and admissible trajectory deforma- 
tions 

Let us consider a commanded system of dimension N. 
Suppose that n of the system variables form an affine space. 
As an example, consider the unicycle model II 1311 



Let it be a function of 3> Q and consider the following dif- 
ferential equation with discontinuous right-hand side (see J5I]) 



x 

y 



v cos(0) 
v sin(0) 



(1) 



where (v,lu) are the system control inputs (or commands) and 
(x, y, 6), the system variables. The (x, y) space can be viewed 
as an affine space of dimension n — 2. We call (x, y) the base 
variables and the associated affine space, the base space. 

We say that a full-space trajectory C(t) te mx] — 
(x(t),y(t),9(t)) in the above example) is admissible if one 
can find a set of admissible commands {v and uj in the exam- 
ple) that generates C. A base-space trajectory C (C = (x, y) in 
the example) is admissible if there exists an admissible full- 
space trajectory whose projection on the base space coincides 
with C. 

Let C(i)te[o,T] be a base-space trajectory and r G [0,T], a 
given time instant. We say that a transformation T occurring 

at r deforms C(i) te [ ,r] into C'(i)te[o,T] if 

Vt < r C'(t) = C(t) 
Vt>r C'(t) = F(C{t)). 

Given an admissible base-space trajectory C, an affine 
transformation T is said to be admissible if T deforms C 
into an admissible trajectory. 

C. Differential equations with discontinuous right-hand sides 

For convenience, we denote by S>° the space of piecewise 
continuous functions with finite limits at the discontinuity 
points - or piecewise C° functions (see Fig. Q] top plot, for an 
example). Typically, the linear acceleration of a mobile robot 
would belong indeed, any brusque press on the throttle 
or on the brake pedal would correspond to a discontinuity of 
the linear acceleration. 




x(t) 
x(0) 



u{t) 
x 



(2) 



It follows from the definition of 0° that any solution x 
of system (01 is C° and piecewise C 1 . Conversely, for any 
function x which is C° and piecewise C 1 , one has x G 23° . For 
convenience, we denote by S) 1 the space of such x functions 
(see Fig. [T] bottom plot, for an example). 

Finally, we denote by Q) 2 the space of differentiable func- 
tions whose derivatives are in This definition does not 
involve technical difficulties since the functions of 3l x are 
continuous. 

In the unicycle example of section IH-BI if the linear and 
angular accelerations a and u> are assumed to be in & a then 
the linear velocity v and the orientation 9 will belong to 
which implies in turn that the position (x,y) belongs to 2$ 2 . 

D. Dimension of the space of admissible affine deformations 

From the previous section, one can see that, typically, some 
of the variables are required to be continuous. These continuity 
conditions are particularly critical at the time instant r when 
the deformation occurs. In general, if one needs to guarantee 
the continuities of m variables at r, this will define m 
constraints on the set of admissible affine transformations. On 
the other hand, the affine transformations of an n-dimensional 
space form a Lie group of dimension n + n 2 (n coordinates 
for the translation and n 2 coordinates for the endomorphism 
of the associated vector space, where n is the number of base 



variables). Consequently, if n 



> m, one could expect 



Fig. 1. Examples of functions of S>° (top) and of S* 1 (bottom). Note that 
the top function is actually the derivative of the bottom function. 



to have at our disposal r and n + n — m "extra degrees 
of freedom" to achieve the desired correction while staying 
admissible. 

For wheeled robots of class I (see section Ull-Ct and wheeled 
robots of class II (section ILII-DI ). the base space (x,y) is of 
dimension n = 2. We show that there are respectively m = 4 
and tyi = 5 continuity conditions for these systems, yielding 
respectively n + n 2 — m = 2 and n + n 2 — m — 1 "extra 
degrees of freedom". We then suggest how to play with r 
and these "extra degrees of freedom" to make corrections 
towards virtually any desired final position and orientation. For 
the tridimensional underwater vehicle (section IIVK the base 
space (x,y, z) is of dimension n = 3 and there are m = 6 
continuity conditions, yielding n + n 2 — m = 6 "extra degrees 
of freedom". 

III. Affine trajectory correction for planar 

WHEELED ROBOTS 

The above presented framework suggests the following 
general scheme to study affine trajectory correction for a 
particular system 

1) check the conditions for a base-space trajectory to be 
admissible; 

2) characterize the set of admissible affine deformations; 

3) compute the admissible affine deformation that achieves 
the desired trajectory correction. 
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To illustrate, let us now apply the above scheme to wheeled 
robots, which constitute an important class of nonholonomic 
systems. 

A. Model description 

At the kinematic level, any wheeled robot whose wheels 
obey the rolling without slipping constraints can be modeled 
by|S 

f £ = Bfoflri 

I P = c 

where £ = (x,y,9) T is the posture of the robot and 
f3 = (f3i . . . f3h) T contains the steering angles of the centered 
orientable conventional wheels (h = if there are no such 
wheels). As in the unicycle example of section HI-Bl we choose 
x and y to be the base variables. The base space is thus of 
dimension 2. The non-base variables are 9 and f3i . . . (3^. 

Throughout this section [TTU we assume, to avoid singulari- 
ties, that the linear velocity y/ x 2 + y 2 of the robot is always 
strictly positive. 

The commands of the system are given by 77, which contains 
the linear velocities of well-defined reference points on the 
frame of the robot, and (, which contains the rates of change 
of the steering angles of the centered orientable wheels. We 
assume that the commands obey the following conditions 

• the space of admissible commands 77 is Sr. This is 
consistent with the fact that the linear accelerations a of 
the reference points, which are the derivatives of 77, are in 
S°. The possible discontinuities of a would correspond 
to e.g. brusque presses on the throttle or on the brake 
pedal; 

• the space of admissible commands £ is Sft . The possible 
discontinuities of Q would correspond to e.g. hard turns 
of the steering wheel in a car. 

B. Admissible base-space trajectories 

As shown in J^t], any planar wheeled mobile robot can be 
described by one out of the five sets of "forward" kinematic 
equations of Table IIII-BI given a suitable choice of a reference 
point and of a basis attached to the robot frame. 

For each type of robot, we now characterize the admissible 
base-space trajectories given the spaces of admissible com- 
mands assumed in the previous section. The reader is referred 
to Table lHl-Bl for the necessary notations and equations. 

1) Type (3,0): Consider (771, r) 2 , m) € S 1 . The third 
"forward" kinematic equation (9 = 7/3) implies that 9 G S 2 '. 
The first and the second forward equations then imply that 
x G S 2 and y G S 2 . 

Conversely, consider a base-space trajectory C = (x, y) G 
S 2 . One can choose an arbitrary function 9 6 S 2 and then 
compute (771,772,773) G S 1 by the "reverse" equations. 

In summary, a base-space trajectory of a (3,0) wheeled robot 
is admissible if and only if it belongs to S 2 . 

2) Type (2,0): Consider (771,772) G S 1 . As previously, the 
forward equations imply that x and y belong to S 2 . 

Conversely, consider a base-space trajectory C = (x, y) G 
S 2 . One can then compute 9 by the first reverse equation 



9 = arctan2(y, x) where 

( tt/2 if a = and b > 
arctan2(6, a) = < — n/2 if a = and b < . 

[ arctan(6/a) if a ^ 

Remark that the so-calculated 9 belongs to Si, but not 
necessarily to Of 2 . Next, one can compute 772 by the third 
reverse equation. For 772 to be in S 1 , one would need 9 G S 2 . 
As just remarked, the latter condition is not automatically 
guaranteed by C — (x, y) G ( 3) 2 . On the other hand, demanding 
that C = (x,y) G f^ 3 would be unduly restrictive. Thus 
the condition 9 G 2t 2 must be specified as an independent 
supplementary condition. 

In summary, a base-space trajectory C of a (2,0) robot is 
admissible if and only if it belongs to Si 2 , and if the function 
9 - as computed from C by the first reverse equation - also 
belongs to S 2 . 

Note that these admissibility conditions can also be for- 
mulated in terms of continuity constraints on the path curva- 
ture nn. 

3) Type (2,1): Consider (771,772) G S 1 and ( G The 
third and fourth forward equations imply that 9 and f3 belong 
respectively to S 2 and S 1 . Next, the first and second forward 
equations imply that x and y belong to &> 2 . 

Conversely, consider a base-space trajectory C = (x, y) G 
& 2 . One can choose an arbitrary function 9 G 2> 2 and then 
compute successively /3 G ffl, (771,772) G and Q G by 
the reverse equations. 

In summary, as for (3,0) robots, a base-space trajectory of 
a (2,1) robot is admissible if and only if it belongs to 'Si 2 . 

4) Type (1,1): As previously, a necessary condition for the 
admissibility of a base-space trajectory is that it belongs to 
S> 2 . Conversely, consider C = (x,y) G £) 2 . The first reverse 
equation allows to compute 9 G 9r . Remark that, as for (2, 0) 
robots, the so-calculated 9 does not necessarily belong to < 2> 2 . 
Next, (3 can be computed from the second reverse equation. 
Remark that the derivative of 9 is used in the computation 
of j3, such that j3 belongs to but not necessarily to Si 1 . 
However, in order to compute next (, one needs j3 G S 1 , and 
consequently 9 G S 2 . 

In summary, as for (2,0) robots, a base-space trajectory C 
of a (1,1) robot is admissible if and only if it belongs to S> 2 , 
and if the function 9 - as computed from C by the first reverse 
equation - also belongs to 2> 2 . 

5) Type (1,2): This type of robots can be treated in the 
same way as (3,0) and (2,1) robots. A base-space trajectory 
of a (1,2) robot is admissible if and only if it belongs to S 2 . 

6) Summary: Following the previous development, one can 
divide wheeled robots in two classes. Class I comprises robots 
of type (3,0), (2,1), and (Ij2), or in other words, those whose 
degrees of maneuvrability [3] equal 3. A base-space trajectory 
for robots of this class is admissible if and only if it belongs 
to 2> 2 . 

Class II comprises robots of type (2,0) and (1,1), or in 
other words, those whose degrees of maneuvrability equal 2. 
A base-space trajectory C = (x, y) for robots of this class is 
admissible if and only if it belongs to S> 2 and if the function 
9 = arctan2(?), x) also belongs to S> 2 . 
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TABLE I 

Forward and reverse kinematic equations for planar wheeled robots 



Type 


Examples 


"Forward" kinematic equations (cf [3J) 


"Reverse" equations 


Admissibility cond. 


(3,0) 


Omni- 
directional 
robots 


(x,y)< = R(fl)(,^)' 

o = m 

where R(fl) = ( COS * " sin / ) 
w I sm6 cos J 


9 e & 2 (arbitrary) 

(t)i,t) 2 ) t = R(S)- 1 (i,y) T 

7)3 = 


(x,y) G S? 2 


(2,0) 


Two- wheel 
differential 
drive 


x = — 771 sin # 
y = r)i cos 9 

= m 


9 — arctan2(y,x) 
7)1 = yjx 2 +y 2 
7)2 = 9 


(x,y)£& 2 
arctan2(y,±) £ f^ 2 


(2,1) 


Unicycle 


x — —7)1 sin(6 + /3) 
y = 7)1 cos(6 + p) 

= 7)2 

$ = Ci 


9 £ & 2 (arbitrary) 
P — arctan2(y, x) — 9 

7)1 = \/x 2 + y 2 

T)2 = 9 

Ci = P 


(x,y) G S> 2 


(1,1) 


Bicycle, 
kinematic 
car 


x — — 7)iL sin 9 sin/3 
y = 77iLcos0sin/3 
6 = t)i cos /3 

4 = Ci 


9 — arctan2(y, ±) 
P = arctan2(y/(Lcos6»),#) 
7)1 = ^x 2 + y 2 /(L sin P) 
Cl = $ 


(x,y) G S> 2 
arctan2(y,±) G & 2 


(1,2) 


Kludge 
robot 
(cf [3J) 


x — — 7)i(2L cos 9 sin /3i sin/32 
+Lsin6»sin(/?i + /3 2 )) 

y = —771 (2L sin 6 1 sin /3i sin P2 
-Lcos9sin(Pi + /3 2 )) 

= r?i sin(/3 2 - Pi) 

ft = Ci 

ft = C2 


9 e S> 2 (arbitrary) 
Pi — arctan2(i cos 9 + y sin 9, 

2L9 -xsin9 + y cos 9) 
Pi — arctan2(i cos 9 + y sin 9, 

-2L9 - x sin 9 + y cos 9) 

7)1 = e/sm{p 2 -Pi) 
Ci = ft 

C2 = ft 


(x,y) € Si 2 



Important remark: From a computational viewpoint, if 
one obtains an admissible base-space trajectory C'(t) t £io,T] 
(for instance by deforming a given C(i) te [ 0j T]), the reverse 
equations allow to easily compute the commands that gen- 
erate that trajectory by some differentiations and elementary 
operations. A 

Relationship with flatness theory: Our approach here 
bears some resemblance with flatness theory [6]. In both cases, 
a reduced set of variables is manipulated (here: the base vari- 
ables; in flatness theory: the flat outputs) and the state of the 
other variables are subsequently recovered from this reduced 
set (here: using the reverse equations). There are however 
two important differences. First, in our approach, certain non- 
base variables, in some systems, are not computed from the 
base variables but chosen arbitrarily: e.g. the orientation 9 in 
wheeled robots of class I (see above) or the roll angle <fi in 
the underwater vehicle (see section IIV-BI i. Second, in some 
systems, certain non-base variables are computed from the 
base variables using integration: e.g. the orientation 9i (i > 0) 
of the trailers (see section IV- Al l. In flatness theory, all non- 
base variables must be computed from the flat outputs, and 
they must be done so using only differentiations and algebraic 
operations. 

Finally, note that it could be interesting to study affine 
deformations of the trajectories of the flat outputs. A 

C. Class I robots 

We now characterize the affine deformations that preserve 
the admissibility of base-space trajectories for robots of class I. 
Using this characterization, we then suggest practical algo- 
rithms for trajectory correction. 



1) Admissible deformations: Consider an admissible base- 
space trajectory C(t)te[o.T] an d an affine deformation T oc- 
curring at time r that deforms C into C . In what follows, 
we note v — y/ x 2 + y 2 (the linear velocity of the robot) 
and 8 — arctan2(y, x) (its orientation). Note that, following 
section IHI-B61 C is admissible if and only if (x, y) £ 2l 2 , i.e., 
if and only if (v,9) £ . Note also that the function 9 here 
is not the same as the 9 chosen arbitrarily in Table IIII-BI For 
instance, the unicycle described by equations ((TJ is a in fact 
a (2,1) robot, with the following correspondance between the 
variables 

#robot = 
/^robot — ^unicycle 7T/2 
( ^lrobot = f un i C y C le . (3) 

J l2 robot = 
Crobot ^unicycle 

One has first, by definition, C (i)t£( T ,T] = •^ r (C(i)te(r,T])- 
Since J 7 is a smooth application, it is clear that C (t) t <z( Tt T\ 
- note that the interval is open at r - is in S! 2 if and only if 

C(f) t6 (r,r| is in 

Regarding the time instant r, the continuities of x and y 
impose that J r (C(r)) = C(r). Thus T can be written in the 
form 

VP 6 A F(P)=C(t)+M(C(t)P). (4) 

One now needs to guarantee the continuities of v and 9 at r, 
since the two remaining conditions (differentiability and finite 
limits for the derivative) do not depend on the behavior of C 
at the discrete point r, and are therefore already satisfied by 
virtue of the smoothness of T. 
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Consider the velocity vector v = (x,y) T . Remark that the 
continuity of this velocity vector is equivalent to those of v and 
9. The continuity of v means that v(r— ) and v(r+) (where 
the signs — and + denote respectively the left and right limits) 
are well defined, and that v(t— ) = v(r+) = v(r). 

Similarly, the continuity of v' would mean v'(r+) = 
v'(r— ) = v(r). On the other hand, one has v'(r+) = 
A4(v(t)). These equalities together imply A4(v(t)) = v(r). 

Let us now decompose M is the basis {uii,Ux} where 
u II = (cos(#), sm(9)) T is the unit tangent vector and = 
(— sin(0), cos(#)) T is the unit normal vector. The condition 
M(v(t)) = v(r) is equivalent to 



A 

l + /x 



(5) 



where M is the matrix representing M in the basis {u||, u_l}. 

In summary, the admissible affine transformations at time r 
form a Lie group of dimension 2, parameterized by A and /i 
in equation (|5]l 

2) Trajectory correction: We consider only the correction 
of the final position and assume that r is given. It is possible 
to achieve more complex corrections (e.g. correcting the final 
orientation) or to choose "optimal" rs: these developments are 
left to the reader. 

From equation to correct the final position C(T) towards 
a desired position P c i = (xd, yd), one needs to look for a linear 
application A4 such that 

M{C{T)C(T))=C[r)P f d . (6) 

Let Q = [u|| , uj_] and let the matrix representing M. in the 
basis {ii||, uj_} be 



M = 



1 A 



1 + n 
Equation © implies 

-l ( X ( T ) - X ( T ) 



Let next 



Hi 



xi 

i)2 



Q 



Q 



x(T) — x(t) 

y( T ) - y( T ) 



, / x d - x(t) 



Vd - y(r) 



Equation (O then implies 



A = (x 2 - x x )/yi, n = (j/ 2 - Vx)/yi, 

provided that y\ ^ 0, i.e. that the tangent at r does not go 
through C(T) (see also discussion in section ITH-D2I) . Fig. [8] 
shows examples of trajectory corrections for the unicycle. 

Note that any desired position in the whole space - and not 
only those in the vicinity of the initially planned final position 
as in [ 19] - can theoretically be reached. Remark on the other 
hand that the distance (e.g. the L 2 distance) of the corrected 
trajectory from the original trajectory is a continuous function 
of A and fi, meaning that using small As and /is results in small 
changes in the overall trajectory (and in the commands). 



D. Class II robots 

1) Admissible deformations: Consider an admissible base- 
space trajectory C of a class II robot and an affine deformation 
F occurring at time r that deforms C into C. In what follows, 
we note v — x 2 + y 2 (the linear velocity of the robot), 
9 = arctan2(y, x) (its orientation), and lo — 9 (its angular 
velocity). Note that, following section |TTl-B61 C is admissible 
if and only if v £ 3> x and us £ 

Following the same reasoning as in section IIII-CU one can 
show that C'(t) fe ( r T ] is in 2$ 2 if and only if .F(v(t)) = v(r), 
where v(r) is is the velocity vector at r. One now needs 
to check the continuities of ui' at r and at the discontinuity 
points of the second derivative of C (the continuity and 
differentiability of u/ elsewhere are already guaranteed by the 
smoothness of T, cf. section ITlI-C II ). 

Consider for this the acceleration vector a = (x, y) T . By 
definition, one has 

a = aii|| + iwi, 
with a not necessarily continuous. One can next write 

a • = vu. (8) 

Consider now a time instant t > r when a is possibly 
discontinuous, that is a(t— ) ^ a(t+). Since u and v are 
continuous, one has by equation (0 

a(t-) • ui(t) = a(t+) • uj_(t), 

or, in other words, that a(t+) — a(t— ) and U|| (t) are collinear. 
Here comes into play a nice property of affine transformations: 
they preserve collinearity. Using this property, one obtains 
that M(a.(t+) - a(t— )) and M(u||(t)) are collinear. But the 
former vector is no other than a'(t+) — a'(t— ) and the latter 
is collinear with u'| (t), since 

, _ Mju^t)) 
U « W ' ||M(n B (*))||- 

Thus a'(t— ) • u' ± (t) = a'(i+) • u' ± (t), which in turn implies 
the continuity of us' at t (note that this conclusion also relies 
on the fact that v' is nonzero if v is nonzero, owing to the 
non- singularity of Ai). 

Remark: Since the affine group is the largest transformation 
group of the plane that preserves collinearity, the previous 
development shows that it is also the largest group that 
preserves the admissibility of every trajectory of a class II 
robot! A 

Turning now to the time instant t, the same reasoning as 
previously shows that us' is continuous at r if and only if 

a'(r+) • u_l(t) = a(r) • u±(r), 



or equivalently, if 

M(a(r)) • ul(t) = a(r) • u±{r). 



(9) 



Remark now that, since v • = 0, condition (O is in fact 
equivalent to 

3A e M M(a(r)) = a(r) + Av(r). 



Denoting by B the linear application such that £>(v(r)) = 
and £>(a(r)) = v(r) (one can compute B explicitly by B = 
[0, v(t)][v(t), a(r)] -1 ), one obtains 



3X € 



M=1 + XB, 



where I is the identity application. 

In summary, the admissible affine transformations at time t 
form a Lie group of dimension 1, given by {I + XB}\ & k. 

Inflection points: The previous development is valid only 
when v(t) and a(r) are non-collinear, that is, when C(t) is not 
an inflection point (see also JH for an interesting discussion 
on inflection points in the context of human movements). A 

2) Trajectory correction I: position correction using one 
affine deformation: Let us now play with r and the "extra 
degree of freedom" A to make trajectory corrections. 

For this, we first study how the final position of the 
trajectory C(T) is affected by an admissible affine deformation 
occurring at time r. By definition, one has 



C'(T) 



C(r)- 
C(T) 



(I + XB)(C(t )C(T)) 
■XB(C(t)C(t)). 

in the (in general non- 



Let us decompose C(t)C(T 
orthonormal) basis {v(r),a(r)} 

C(t)C{t) = 7 v(t) + 5a(r). 

By definition of B, one has 

C'(T)=C(T) + X8v(t). (10) 

Consequently, if S is nonzero (that is, if C(t)C(t) and v(t) 
are non-collinear, or in other words, if the tangent at r does 
not go through C(T)), then the locus of C'(T) when A varies 
is the line that goes through C(T) and that collinear with v(t). 

In order to make a correction of the final position from C(T) 
to a desired position Pd, it therefore suffices to 

1) compute the vector e,j = C(T)Pd\ 

2) find a time instant t when the tangent uy (r) is collinear 
with e^; 

3) compute A = ed/(<5v(r)) where the overline denotes 
the signed norm; 

4) make the affine deformation of parameter A at time r. 
Fig. |2] shows some examples of trajectory correction for a 

kinematic car, which is a robot of type (1,1). The equation of 



a kinematic car is given by II 1311 



X = 


v cos(9) 


is = 


vsin(8) 


9 = 


v tan(/3) 




L 


13 = 


c 



(11) 



which can be put in the form of a robot of type (1,1) (cf. 
Table IIII-Bb using the following correspondance between the 
variables 




0car " 7T/2 

tt/2 - /3 car 
/(Lcos/? C£ 

Ccar 



(12) 
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Accessible positions: From the previous development, it 



Fig. 2. Accessible final positions (in cyan) and two examples of position 
corrections. The original trajectory is in red. For each correction, the black 
plain line represents the tangent at r while the black dotted line joins 
the original final position C(T) to the desired final position P^. Note the 
collinearity of the plain line and the dotted line. 



appears that a position is accessible if and only if the 
original trajectory C(t) te [o.T] nas a tangent that is parallel to 
C(T)Pd- Therefore the set of the trajectory tangents (minus 
the tangents at the inflection points) determine the accessible 
directions for position corrections, as shown in Fig. A 

3) Trajectory correction II: orientation correction using one 
affine deformation: Remark that, if 5 = in equation ( [Tol l, the 
final position C(T) does not move when A varies. However, 
the final orientation does vary with A. Exploiting this fact, one 
can make corrections to the final orientation without changing 
the final position. 

As remarked earlier, 6 — when C(t)C(t) and v(r) are 
collinear, that is, when the tangent line at time r goes through 
C(T). Consequently, in order to make a correction of the final 
tangent vector from u.| (T) to a desired tangent vector 
while keeping the final position unchanged, it suffices to (see 
Fig. EK) 

1) find a time instant r such that the tangent line at r goes 
through C(T); 

2) compute the appropriate A (see below); 

3) make the affine deformation of parameter A at time r. 

Computation of A: Remark that the final orientation of 
the deformed trajectory is given by the vector _M(u|| (T)). 
Observe next that 

7W(u||(T)) = U||(T) + Ar5„v(r) 

where 5 U is the coefficient multiplying a(r) in the decompo- 
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B 




-5 5 10 15 20 25 

Fig. 3. A: accessible final orientations (in cyan) and two examples of 
orientation corrections. The black line represents the tangent at t. Note that 
the black line goes through the final position, which remains unchanged by 
the orientation corrections. B: illustration for the computation of A in the 
correction of the final orientation. 



sition of uii(T) in the basis {v(r),a(r)}. 

Consider the intersection I between the line containing 
and the line parallel to v and which goes through the tip 
of U|| (T) (see illustration in Fig. [3}}). The directed distance 
between / and the tip of U||(T) is given by 



sin(v(r),U||(T)) 



tan(v(r),u rf ) 
The appropriate A must then satisfy 



CO S (v(r),U||(T)) 



\5 u v(t) = d, 



which leads to A = d/ (5 u v(r)). A 

Accessible orientations: The accessible orientations are 
restricted to the half-circle defined by the tangent line and 
in which lies 6(T), as shown in Fig. [3}\. Note that different 
choices of the tangent lines (when there exist more than 
one possible tangent line) induce different sets of accessible 
orientations, whose union forms the total set of accessible 
orientations. Note that the tangents at the inflection points are 
also forbidden here. A 

4) Trajectory correction III: position correction using two 
affine deformations: One can in fact compose several affine 
deformations to achieve more powerful trajectory corrections. 
In particular, composing two deformations allows making 
position correction towards any desired final position in space, 
so long as the initial trajectory C is not a straight line, as 
follows (see Fig. |4]i 

1) select two (non-inflection) time instants t\ and t 2 , with 
T\ < t 2 , such that v(ti) and v(t2) are non-collinear. 
Such two time instants exist since C is not a straight 
line; 

2) decomp ose C(T)Pd in the basis {v(ti), v(t2)} as 
C{T)P d = a 1 v{n) + a 2 v(T 2 ); 

3) apply a first deformation on C at t 2 to obtain C, with 
C'{T)=C{T)+a 2 v{T 2 )- 

4) apply a second deformation on C at T\ to obtain C", with 
C"{T) = C'(T) + aiv(Ti). By construction C"(T) = 
C(T) + a 2 v(r 2 )+a 1 v(r 1 ) = P d . 
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Fig. 4. Position correction using two successive affine deformations. Note 
that the desired final position at P<j = (20, 40) is not accessible by any single 
deformation because the initial trajectory C (red) has no tangent parallel with 
the line C(T)P l i (yellow line). To overcome this, C is first deformed into C' 
(blue), which in turn is deformed into C" (green). 



It is crucial that the deformation at t 2 is made first (and 
the deformation at t\ only second) so as to leave the velocity 
vector at t\ unchanged (v'(ti) = v(n)). 

5) Trajectory correction IV: position and orientation cor- 
rection using three affine deformations: Composing three 
affine deformations allows achieving both the desired final 
position and orientation as follows (see Fig. ID 

1) select three (non-inflection) time instants n, t 2 , and t 3 
with t\ < t 2 < t 3 , such that v(ti), v(t 2 ) and v(ra) 
are pairwise non-collinear. Such three time instants exist 
since C is not a straight line; 

2) apply a first deformation on C at r 3 to obtain C, with 
C'(T) = C{T) + a 3 v(r 3 ), where a 3 is a coefficient to 
be tuned later; 

3) following the results of the previous section, one can 
use the second and third deformations to correct back to 
the desired position C"'(T) = Pd- Remark that the final 
orientation of C" depends on a 3 as shown in Fig. [5] The 
formula to algebraically compute a 3 as a function of the 
desired final orientation can be obtained in a similar way 
as in section ITH-D3I 

Finally, remark that one can also set the final linear speed to 
arbitrary values while keeping the final position and orientation 
unchanged by using the extension technique of section IV-DI 
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always strictly positive and that the trajectory stays away from 
the singularities of the Euler angles IU6I0 



V 

e 



arctan2(y, x) 



R( 



2 + y 2 + z 2 
arcsin(i/t;) 

i> 

6 



In summary, a base-space trajectory is admissible if and 
only if it is in <2> 2 . 



Fig. 5. Position and orientation correction using three successive affine 
deformations. The left and right plots correspond to two different values 
of a:j. Remark that the trajectory C" (magenta) ends at the same position 
(P d = (20, 40)) in the left and right plots, but that its final orientation differs 
significantly between the two plots. By varying as, it is thus possible to cover 
a large range of possible desired final orientations while keeping the desired 
final position fixed. 



IV. Affine trajectory correction for a 

TRIDIMENSIONAL UNDERWATER VEHICLE 

A. Model description 

A tridimensional underwater vehicle can be modeled by the 
following equations floll 



x 

y 



V COS if) cos 9 
v sin ip cos 9 
—vsmO 



Uy 



where (v, tu x , uj y , lu z ) are the system 
(x, y, z, <f>, 9, tp) the system variables, and 



(13) 



commands, 



K(4>, 




sin (f> tan ( 

cos cj> 
sin d> sec t 




We choose x, y, and z to be the base variables. The base 
space is thus of dimension 3. The non-base variables are <fi, 9, 
and ip. 

As in the case of planar wheeled robots, the admissible 
commands v are assumed to be in S 1 (allowing possible 
discontinuities in the linear acceleration). The admissible 



commands uj 7 



and oj z are assumed to be in S>° . 



B. Admissible base-space trajectories 

Following the same line of reasoning as previously, a neces- 
sary condition for the admissibility of a base-space trajectory 

C(t)te[o,T] = (x(t),y(t),z(t)) te[a ^ T] is that x, y and z belong 
to Si 2 . 

Conversely, assume that x, y and z belong to S 2 . Remark 
first that, from the system equations (TTTt . the "roll" angle cf> 
is independent of (x(t), y(t), z(t)) t ^[o,T]- Next, given an arbi- 
trary roll angle profile <fi(t)t£[o,T] G one can safely write 
the following reverse equations (assuming that the velocity is 



C. Admissible affine deformations 

Consider now an admissible base-space trajectory C and an 
affine deformation T occurring at time r that deforms C in 
to C. As in section IIII-CU one can show that C'(t) te ( T t] 
belongs to £> 2 , owing to the smoothness of T. 

At the time instant t, the continuities of x, y and z impose 
that J r (C(r)) = C(r). Thus T can be written in the form 



VPeA T{P) =C(t)+M(C(t)P). 



(14) 



Next, following again the same reasoning as in sec- 
tion IIII-CU the continuities of v, tp and 9 are equivalent to 
setting M(v(t)) = v(r). 

In summary, an affine deformation T occurring at time t is 
admissible if and only if M(v(t)) = v(r) when T is written 
in the form ( fl4l i. As a consequence, the admissible affine 
transformations at time r form a Lie group of dimension 6. 

In practice, we shall compute A4 in the basis {u||, wi, W2} 
where wi and W2 are two arbitrary unit vectors forming 
an orthonormal basis with U||. In this basis, the condition 
M(v(t)) = v(t) is equivalent to setting the first column 
of the matrix that represents Ai to (1,0,0). It suffices then to 
find the six remaining coefficients. 

D. Trajectory correction 

We consider only the correction of the final position, at a 
given r. It is possible to achieve more complex corrections as 
well (correcting the final orientation, avoiding obstacles, etc.) 
or to optimize the time instant r: these developments are left 
to the reader. 

Theoretically, three free coefficients are sufficient to reach 
any final position. As a consequence, we have here more 
coefficients than needed. We solve this "redundancy" problem 
by choosing an affine transformation that is the "closest" to the 
identity matrix, i.e., that affects the least the original trajectory. 

As in section IIII-C2I to correct towards a desired position 
Pd = (xd,Ud, Zd), one needs to look for a linear application 
M. such that 

M(C(t)C(t)) = C(r)jt (15) 

Let Q = [u||,wi,W2] and let the matrix representing Ai in 
the basis {u||,wi,W2} be 
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Equation < TT3T > implies 

x(T) 
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z(T) 
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Equation ( fT6] l then implies 
( A \ 
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(17) 



where 



U = 




The (A, /x, v : £, a, x) with minimal norm (i.e. that yields a 
Ai closest to identity according to the Frobenius distance) and 
that satisfies the under-determined system (flTt is given by 




where U + denotes the Moore-Penrose pseudo-inverse of U. 

Finally, one needs to choose the "independent" angle 
(f>(t) t£ [ T T]- Here our strategy consists of keeping the same 
(f> as in the original trajectory. Other strategies (e.g. keeping 
the same absolute roll as in the original trajectory) can also be 
used. Fig. [6] shows some examples of trajectory corrections. 

V. Further applications 
We now use the trajectory correction algorithms just devel- 
opped as basic tools to tackle more complex tasks. We mostly 
use the kinematic car (which is a wheeled robot of type (1,1) 
and class II, see section ILU-B4I) as illustrative example but 
the following developments can be easily adapted to other 
nonholonomic systems, provided that affine corrections are 
available for these systems. 

A. Wheeled robots towing trailers 

A kinematic car towing p trailers can be modeled by 
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and for i = 1 . 
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Fig. 6. Examples of trajectory corrections for an underwater vehicle. The 
original trajectory is in red. 



where (v, Q are the system commands (respectively the linear 
velocity of the car and the rate of change of the steering angle) 
and (x, y, (3, 0q, 9\, . . . n ), the system variables (respectively, 
the x and y coordinates of the car in the laboratory reference 
frame, the steering angle, the angle of the car with respect 
to the laboratory reference frame, the angle of the first trailer 
with respect to the laboratory reference frame, etc.). 

The same reasoning as in the case of the simple kinematic 
car shows that a base-space trajectory C = (x, y) is admissible 
only if it belongs to 2i 2 and if 9q - computed from C by 
f?o = arctan2(y,±) - belongs to S3 2 . Conversely, assume that 
C is in < 3 2 and #o - computed from C - is in < 3 2 . One can then 
safely compute v £ <%l x , (3 G £F\ 9 S & 2 (by assumption) 
and C G as in the case of the simple car. Next, to obtain 0i 
(for i = 1 . . . ri), it suffices to solve successively the following 
(ordinary) differential equations 




cos 



'j-l 



sin 



-9i) 



(18) 



In summary, the set of admissible base-space trajectories of 
a car towing p trailers is the same as that of a simple car. 
As a consequence, the admissible affine deformations and the 
trajectory correction algorithms for a car towing p trailers 
are also the same as those for a simple car. An example of 
trajectory correction for a car towing two trailers is given in 

Fig.m 

Note that we have no "control" over the configurations 
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of the trailers, contrary to the literature (transformations to 
chained forms lfl5l 12011 . flatness theory H, etc.)- However, 
consider the (commonly encountered) case when the end of 
the initially planned trajectory consists of a straight segment, 
in order to align the trailers with the car. Since affine transfor- 
mations preserve collinearity, the end of the corrected trajec- 
tory will also consist of a straight line, which automatically 
guarantees the alignment of the trailers with the car. 

Note finally that it could be interesting to study affine 
deformations of the trajectory of the flat output H, which 
is, in the present case, the middle of the rear wheels axle of 
the last trailer (assuming that each trailer is hooked up at the 
middle of the rear wheels axle of the preceding trailer). 
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2) select a new, non colliding, intermediate position 
(^inter, Winter) to which one could make a correction; 

3) make the correction of (x(t b s ), y(t obs)) towards 
Winter, 2/intcr), using r (s) < i obs ; 

4) re-correct the final position towards the initially planned 
final position, using r(s) > t t, s . 
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Fig. 8. An example of obstacle avoidance for the unicycle. The original 
trajectory (x,y) (red) was planned knowing the position of the black 
obstacles. During the execution, an unforeseen obstacle (cyan) appears on the 
original path. A new trajectory (xi,yi) (blue) is obtained by deforming the 
original trajectory. The blue star indicates the position (rr(ri), j/(ti)) where 
the deformation occurs, and the black plain line joins (x(t b a ), j/(t bs)) to 
(xinten 3/intcr). Next, in order to get back to the original target, an other 
trajectory (green) is obtained by deforming the blue one. The green star 
indicates the position (xi (ts), J/i (ts)) where the deformation occurs, and 
the black dashed line joins (x 1 (T),y 1 (T)) to (x(T),y(T)). 



Fig. 7. Trajectory correction for a car towing two trailers. One can imagine 
the following scenario: a trajectory C (red) is initially planned to park the 
car with trailers in a given parking slot; following the occupation of that 
parking slot, a new trajectory C" (magenta) is obtained by deforming the 
red trajectory using the algorithm of section ITll-D5l (where the blue and green 
trajectories correspond respectively to C' and C"). The new trajectory allows 
the car to be parked in a neighboring slot, with the same final orientation. 
Note that the collinearity-preserving property of affine transformations auto- 
matically guarantees the straightness of the final segments of the blue, green 
and magenta trajectories, which in turn implies the alignment of the trailers 
with the car. 



B. Obstacle avoidance 

In the trajectory correction algorithms previously devel- 
opped, one can in fact replace the final time T by any time 
instant t > r. This allows implementing interactive obstacle 
avoidance algorithms as follows 

1) determine a time instant i b s when the initially planned 
trajectory would collide with the obstacle; 



This algorithm can be run iteratively to avoid all obstacles. 

One can also prescribe a specific position/orientation of the 
trajectory at a given time instant idoor (this is desirable for 
instance when two large obstacles are close to each other, 
leaving between them a small doorway through which the 
robot could go), as follows 

1) make the correction of (a;(idoor), y(idoor)) towards the 
specified intermediate position; 

2) make the correction of 0(idoor) towards the specified 
intermediate orientation; 

3) re-correct the final position towards the initially planned 
final position, using r(s) > t^ OOT . 

C. Feedback control 

So far, we have been focusing on perturbations affecting 
the state of the target (position and/or orientation) or the 
environment (unexpected appearance of obstacles). Here we 
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show, through a simplified feedback control algorithm, how 
affine corrections can also be used to deal with perturbations 
affecting the robot's own state. 

Consider again the example of the kinematic car. Assume 
that a trajectory has been initially planned (black trajectory 
in Fig. |9]\), in terms of the time series of the control 
inputs (a p ian (t)te[o,T], Cpian(*)te[o,T])- Assume now that these 
control imputs are corrupted by random perturbations 

a(t) = Opu^i) +£i(f) 

C(*) = CplanW+6W ' 



V* e [0, T] 



where £1 and £2 two piecewise constant random functions. The 
red trajectories in Fig. |9]\ represent several trajectories of the 
car corresponding to different realizations of the pertubations 
£1 and £2- One can notice that the perturbations make the final 
positions of the red trajectories deviate randomly from the 
target (denoted by the magenta dot). This can also be noted 
from the variability profile (red curve in Fig. |9j3), which is 
nonzero at the end of the movement. 
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Fig. 9. Feedback control using affine corrections. A: uncorrected sample 
trajectories (red), corrected using at most one correction (blue) or at most five 
corrections (green). The initially planned trajectory is in black. B: variability 
profiles computed across 2000 realizations of the random perturbations £1 
and £2- 



We propose the following feedback control algorithm in- 
spired from ll2ll [l8ll . The algorithm maintains at every step two 
time series (a C ur(i)te[o,T]> Ccur(*)ie[o,T]) termed "currently 
planned control inputs". These time series are initialized at 
the values of (a p ian(*)i£[o,T]> Cplan(*)te[o,T])- The movement 
time T is divided in S + 1 equal parts. At each time instant 
ti = iT I (5 + 1), i = 1 . . . 5, the robot is given the possibility 
to make a correction as follows 

1) compute the final position of the robot, had the con- 
trol inputs (a cm (t)te[u,T],Ccur(t)telti,T]) been applied 
starting at the current state C(tj) and until the end 
of the movement. Denote this final simulated position 



(•■ 



sim ; ysim 



2) compute appropriate trajectory deformations with 
t(s) > ti to correct the final position from 
(x s im, 2/sim) towards (x t arget,2/targct)- This gives rise 
to new time series of control inputs, denoted 

(a n ew(i)te[ti,T], Cncw (i)te[ti,T]); 

3) if the new control inputs are acceptable (i.e. do not 
imply too large accelerations or too sharp turns), set 



Ocur(*)ie[t ( ,T] <- a new(t)te[ti,T] and Ccur(*)te[t«,T] ^~ 
Cnew(i)te[ti,T]- Otherwise, keep the current values of 

a cur and Ccur- 

Figure |9}\ shows the results of the feedback control algo- 
rithm for 5 = 1 (blue curves) and 5 = 5 (green curve). 
Note that the blue and green curves are driven by the same 
realizations of the perturbations as the red curves (uncorrected 
trajectories). However, the blue and green curves end up 
much closer to the target position. Figure |9j3 confirms this 
observation: the final variabilities of the corrected trajectories 
(blue and green profiles) at T are much lower (~1.3m) than 
that of the uncorrected trajectories (~6m). 

One could ask: why make multiple corrections (green) while 
making one unique correction (blue) yields approximately the 
same final average error? Figure [10] shows that 5 = 1 is 
associated with larger values of a, ( and (3 than 5 = 5. 
This is because when the robot is allowed to make multiple 
corrections, the changes to a and £ are distributed instead 
of being concentrated in one single large correction near the 
end of the trajectory. Figure [9j3 confirms this observation: the 
green variability profile (5 = 5) starts decreasing before the 
blue variability profile (5 = 1). Note however that choosing 
5 > 5 does not improve the algorithm. 
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Fig. 10. Statistics of the feedback control algorithm across 2000 realizations 
of the random perturbations £1 and £2- The X-axis represents the maximum 
number of corrections allowed S. The horizontal lines report the values 
corresponding to the uncorrected trajectories (5 = 0). 

Finally, note that this algorithm is not a trajectory-tracking 
algorithm but rather a simplified implementation of "optimal 
feedback control" 112 U Il8ll . 



D. Gap filling for sampling-based kinodynamic planners 

Gap-reduction techniques are a core component of any 
sampling-based kinodynamic planner |4]. As an example, 
consider the approach proposed in [14], which consists of 
growing two rapidly-exploring random tree (RRT) rooted re- 
spectively at the initial state and at the target state - a solution 
trajectory is obtained when these two trees intersect. When 
nonholonomic constraints are present, exact intersections of 
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the trees occur with probability zero, such that one usually 
assumes intersection when the trees are within a nonzero 
distance of each other, yielding thereby a gap in the solution 
trajectory. As the performance of the planner critically depends 
on the permitted gap size (the larger the permitted gap size, 
the quicker the growing trees find an "intersection", but also 
the more difficult filling the gaps), efficient gap-reduction 
techniques have been shown to dramatically improve the 
performance of the planner [4]. 

We now show how affine corrections can be used to fill 
trajectory gaps. Consider two trajectories Ci(i)te[o,Ti] an d 
C2(t)t£[o,T 2 ] °f a kinematic car (respectively in red and cyan 
in Fig. n~TT > separated by a gap. We first "prepare" the two 
trajectories as follows 

1) grow a first stub with time duration A a at the end of 
C\. Using the time interval [Ti,Ti + A a ], bring the 
steering angle j3i to by "counter-steering" (i.e. turning 
the steering wheel back to the straight-ahead position); 

2) grow a second stub with time duration Ab at the end of 
the extended C% (t). During this time interval, the steering 
angle j3\ is kept to 0, resulting in a straight segment. One 
can easily verify that the (doubly) extended trajectory 
£i Wte[o,Ti+A a +At,] is admissible. The two stubs are 
shown by dashed red lines in Fig. [TT] 

3) similarly, grow two other stubs at the beginning of C2 
(shown by dashed cyan lines in Fig. [TTJ. 

After this "preparation", we have two trajectories which 
respectively ends and begins by straight segments. The lengths 
of the added stubs depend on the As and can be made rela- 
tively short if the /3s are small and large braking and counter- 
steering rates are permitted. We can now use the position 
and orientation algorithms given in the previous sections to 
bring the end of the extended C\ towards the beginning of 
the extended C2. Fig. QT] shows an example of such correction 
using three succesive affine deformations (cf. section IIII-D5I ). 
The admissibility conditions are verified by observing that 
> since affine transformations preserve collinearity, the cor- 
rected extended trajectory C'" (magenta) also ends by a 
straight segment. When this straight segment connects 
with the straight segment at the beginning of the extended 
C2, the continuity of (3 is guaranteed; 
• regarding the continuity of v, one can use the straight 
parts around the connection point to modulate the speed 
profile to make it continuous without altering the geo- 
metric path: see the yellow lines in the plots of a and v 
in Fig. ED 

VI. Discussion 

As stated at the beginning of section Hill one can apply the 
following general scheme to study affine trajectory corrections 
for nonholonomic systems 

1) check the conditions for a base-space trajectory to be 
admissible. Often (but not always), a base-space trajec- 
tory is admissible if it - and some functions computed 
from it - belong to certain classes 2> l \ 

2) based on the admissibility conditions of trajectories, 
particularly at the time instant when the deformation 
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Fig. 11. Filling trajectory gaps. Top plot: geometric paths. The original 
trajectories to be connected are showned in plain red line (Ci) and plain cyan 
line (€2)- These trajectories are first "prepared" by growing stubs at their 
extremities (red and cyan dashed lines). The extended Ci is then corrected 
into C"' (magenta) by three successive affine deformations (the blue and 
green lines represent the intermediate trajectories and C" ). Note that C"' 
smoothly connects with C2- Bottom plot: profiles of the other variables. The 
yellow lines in the plots of a and v show the modifications that make v 
continuous without changing the geometric paths. 

occurs, characterize the set of admissible affine defor- 
mations. Often (but not always), the admissible affine 
deformations at a given time instant form a Lie group 
of dimension n + n 2 — m where n is the number of base 
variables and m the number of continuity conditions; 
3) finally, play with t and the n + n 2 — m "extra degrees of 
freedom" to achieve the desired correction. If there are 
more "extra degrees of freedom" than needed, one can 
"optimize" by choosing the affine transformations that 
are the closest to identity. 

This general scheme suggests in turn the classes of systems 
that can or cannot be tackled by the proposed method. For 
instance, an underwater vehicle whose changes in turning rate 
(p x — lj x , p y — ujy, p z — d> z ) are required to be continuous 
could probably be treated by the method (since in this case 
n + n 2 —m = 3). The development of the theory to deal with 
other classes of nonholonomic systems are also the subject of 
ongoing efforts. 

Holonomic systems, such as the end-point of a robotic 
manipulator, are not subject, by nature, to the differential 
constraints with which the current manuscript is concerned. 
However, it is sometimes desirable for efficiency reasons to 
artificially enforce some differential constraints, such as the 
continuity of the velocity vector. For instance, if a planned 
path is not 'tf 1 at some points, the robot must stop-and-start at 
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these points |@], which clearly is an undesired behavior. In this 
perspective, the regularity-preserving deformation algorithms 
developped here can also be useful for holonomic trajectory 
planning. 

As just remarked, this manuscript is mostly concerned with 
the differential constraints that stem from the nonholonomic 
nature of the considered systems. In practice, other constraints, 
such as upper limits on the absolute acceleration or on the 
trajectory curvature, could further restrict the set of admissible 
affine deformations. This can be treated by observing that 
the changes in acceleration or curvature from the original 
trajectory can be computed from the affine transformation at 
hand (see also |Q]]). The integration of such constraints into 
the current framework represents an important task (see e.g. 

Ji). 

Another promising direction of research may consist of 
combining the approach presented here with existing ap- 
proaches for trajectory planning and deformation. We have 
mentioned earlier possible interactions with flatness theory. A 
complementary use of affine-based and perturbation-based de- 
formations 1U2I1 may also lead to more efficient algorithms. For 
instance, affine corrections perform badly when the ori gina l 
trajectory is close to a straight line. Using the results in 01 211 . 
it should be possible to slightly perturb the original trajectory 
to generate local curved portions, which subsequently allow 
applying affine deformations with greater effectiveness. 

As mentioned in the Introduction, one advantage of the 
method presented in this manuscript is that it requires no re- 
integration of the trajectory. On the other hand, differentiations 
of the trajectory must be performed in order to recover 
the commands (see "Important remark" in section HH-B6I 1. 
Note however that, if multiple deformations are made, the 
differentiations need to be performed only once, after all the 
deformations have been applied. 

The group property of affine transformations can also be 
used to further accelerate the computations (as in [19] with 
Euclidean transformations). Assume for instance that two 
affine transformations T\ and T<z are applied at time instants 
T\ and T2, with t\ < t%. Then one can apply T\ to C(t) te r TliT2 i 
and next Ti o T\, which is also an affine transformation, to 
c (t)te(T 2 ,T]- 

Another advantage, also mentioned in the Introduction, is 
that the method presented here can be executed in one step, 
while other methods require iterative deformations of the 
trajectory JT2] or gradient descent to find the appropriate 
deformation coefficients Ji],[lfJ. This may result in significant 
performance gains, in particular, in real-time applications or 
in highly compute-intensive tasks such as the building of 
probabilistic roadmaps [14]. 

Finally, the method is exact: for example, a desired position 
can be reached exactly, and not only approached iteratively "as 
close as we want". This may have important consequences. 
For example, in the initial trajectory planning, one would no 
longer need to spend time finding a trajectory that ends very 
close to the target. Instead, one can plan a trajectory that ends 
roughly somewhere near the target, and then make an affine 
deformation towards the exact target position. 



A last word on the biological implications of the ideas 
presented here. One source of inspiration for the present 
work was indeed the recent studies of affine invariance in 
human perception and movements (see e.g. |Q]] and references 
therein). Conversely, one could ask (and experimentally test) 
whether humans use algorithms similar to those described here 
to correct their hand or locomotor trajectories. 

Acknowledgments 

The author is deeply grateful to Prof. Daniel Bennequin, 
Prof. Yoshihiko Nakamura, and Dr. Oussama Kanoun for 
their highly valuable suggestions and comments. This research 
was funded by an University of Tokyo grant and by a JSPS 
postdoctoral fellowship. 

References 

[1] D. Bennequin, R. Fuchs, A. Berthoz, and T. Flash. 
Movement timing and invariance arise from several ge- 
ometries. PLoS Comput Biol, 5(7):el000426, Jul 2009. 
doi: 10.1371/journal.pcbi.l000426. 

[2] J.D. Boissonnat, A. Cerezo, and J. Leblond. A note on 
shortest paths in the plane subject to a constraint on the 
derivative of the curvature. Rapports de recherche-INRIA, 
1994. ISSN 0249-6399. 

[3] G. Campion, G. Bastin, and B. D'Andrea-Novel. Struc- 
tural properties and classification of kinematic and dy- 
namic models of wheeled mobile robots. IEEE Transac- 
tions on Robotics, 12(l):47-62, 1996. doi: 10.1109/70. 
481750. 

[4] P. Cheng, E. Frazzoli, and S. LaValle. Improving the 
performance of sampling-based motion planning with 
symmetry-based gap reduction. IEEE Transactions on 
Robotics, 24(2):488^194, 2008. ISSN 1552-3098. 

[5] A. Filippov. Differential equations with discontinuous 
righthand sides. Springer, 1988. ISBN 902772699X. 

[6] M. Fliess, J. Levine, P. Martin, and P. Rouchon. Flatness 
and defect of non-linear systems: introductory theory and 
examples. International journal of control, 61(6):1327- 
1361, 1995. ISSN 0020-7179. 

[7] T. Fraichard and A. Scheuer. From Reeds and Shepp's 
to continuous-curvature paths. IEEE Transactions on 
Robotics, 20(6): 1025-1035, 2004. doi: 10.1109/TRO. 
2004.833789. 

[8] M. Hillion and F. Lamiraux. Taking into account veloc- 
ity and acceleration bounds in nonholonomic trajectory 
deformation. In Robotics and Automation, 2007 IEEE 
International Conference on, pages 3080-3085. IEEE, 

2007. ISBN 1424406013. 

[9] F. Kanehiro, W. Suleiman, F. Lamiraux, E. Yoshida, and 
J. P. Laumond. Integrating dynamics into motion planning 
for humanoid robots. In Intelligent Robots and Systems, 

2008. IROS 2008. IEEE/RSJ International Conference 
on, pages 660-667. IEEE, 2008. 

[10] A. Kelly and B. Nagy. Reactive nonholonomic trajectory 
generation via parametric optimal control. The Interna- 
tional Journal of Robotics Research, 22(7-8):583, 2003. 
ISSN 0278-3649. 



14 



[11] M. Khatib, H. Jaouni, R. Chatila, and J.P. Laumond. 
Dynamic path modification for car-like nonholonomic 
mobile robots. In IEEE International Conference on 
Robotics and Automation, 1997. ISBN 0780336127. 

[12] F. Lamiraux, D. Bonnafous, and O. Lefebvre. Reactive 
path deformation for nonholonomic mobile robots. IEEE 
Transactions on Robotics, 20(6):967-977, 2004. doi: 
.829459. 

[13] J.-R Laumond. Robot Motion Planning and Control. 
Springer- Verlag, New York, 1998. 

[14] S.M. LaValle and J.J. Kuffner. Randomized kinody- 
namic planning. The International Journal of Robotics 
Research, 20(5):378, 2001. ISSN 0278-3649. 

[15] R. M. Murray and S. S. Sastry. Nonholonomic motion 
planning: steering using sinusoids. IEEE Transactions on 
Automatic Control, 38(5):700-716, 1993. doi: 10.1109/ 
9.277235. 

[16] Y. Nakamura and S. Savant. Nonlinear tracking control of 
autonomous underwater vehicles. In IEEE International 
Conference on Robotics and Automation, 1992. doi: 10. 
1 109/ROBOT. 1992.220005. 

[17] Q.-C. Pham. Fast trajectory correction for nonholonomic 
mobile robots using affine transformations. In Robotics: 
Science and Systems, 2011. 

[18] Q.-C. Pham and H. Hicheur. On the open-loop and feed- 
back processes that underlie the formation of trajectories 
during visual and nonvisual locomotion in humans. J 
Neurophysiol, 102(5):2800-15, November 2009. ISSN 
1522-1598. 

[19] K. Seiler, S. Singh, and H. Durrant-Whyte. Using Lie 
group symmetries for fast corrective motion planning. In 
Algorithmic Foundations of Robotics IX, 2010. 

[20] O. J. Sordalen and O. Egeland. Exponential stabilization 
of nonholonomic chained systems. IEEE Transactions 
on Automatic Control, 40(l):35-49, 1995. doi: 10.1109/ 
9.362901. 

[21] E. Todorov and M.I. Jordan. Optimal feedback control 
as a theory of motor coordination. Nat Neurosci, 5(11): 
1226-35, November 2002. ISSN 1097-6256. 



